#include "log.h"


static const char hex_asc[] = "0123456789abcdef";
static uint8_t debug_buf[256];

#define hex_asc_lo(x)                   hex_asc[((x) & 0x0f)]
#define hex_asc_hi(x)                   hex_asc[((x) & 0xf0) >> 4]


static inline char *pack_hex_byte(char *buf, uint8_t byte)
{
    *buf++ = hex_asc_hi(byte);
    *buf++ = hex_asc_lo(byte);
    return buf;
}


void bs_writer_print_buffer(void *buf, uint32_t len)
{
    FILE *fp = NULL;
    char *p = (char *)buf;
    while(len-- > 0) {
        fputc(*p++, fp);
    }
}


uint32_t bs_writer_printf(char *fmt, ...)
{
    uint32_t len, res;
    va_list args;
    char *p=(char *)debug_buf;

    res = 0;
    va_start(args, fmt);
    len = vsnprintf(p, sizeof(debug_buf), fmt, args);
    if (len > sizeof(debug_buf)) {
        res = 1;
    }
    va_end(args);
    
    bs_writer_print_buffer(p, len);

    return res;
}


void bs_writer_print_hexdump(void *buf, uint32_t len)
{
    unsigned char *line;
    int i, thisline, offset;
    char* p;

    line = (unsigned char *)buf;
    offset = 0;

    while (offset < len) {
        p=(char *)debug_buf;
        p = pack_hex_byte(p, (offset>>8)&0xFF);
        p = pack_hex_byte(p, (offset   )&0xFF);
        *p++ = ' ';

        thisline = len - offset;
        if (thisline > 16) {
            thisline = 16;
        }

        for (i = 0; i < thisline; i++) {
            p = pack_hex_byte(p, line[i]);
            *p++ = ' ';
        }

        for (; i < 16; i++) {
            *p++ = ' ';
            *p++ = ' ';
            *p++ = ' ';
        }

        for (i = 0; i < thisline; i++) {
            if ((line[i] >= 0x20 && line[i] < 0x7f))
                *p++ = line[i];
            else
                *p++ = '.';
        }

        *p++ = '\r';
        *p++ = '\n';
        offset += thisline;
        line += thisline;
        bs_writer_print_buffer(debug_buf, 5+16*3+ thisline +2);
    }
}
